#include "ibc_nav_obj.h"
#include <stdlib.h>

IBCnav_OBJ_t *IBCnav_create(void)
{
    IBCnav_OBJ_t *nav = (IBCnav_OBJ_t *)malloc(sizeof(IBCnav_OBJ_t));
    if (nav)
    {
        IBCnav_init(nav);
    }
    return nav;
}

void IBCnav_destroy(IBCnav_OBJ_t *nav)
{
    if (nav)
    {
        free(nav);
    }
}

uint16_t IBCnav_checksum(uint8_t *data, uint8_t len)
{
    uint16_t sum = 0;
    for (uint8_t i = 0; i < len; i++)
    {
        sum += data[i];
    }
    return sum;
}

int IBCnav_init(IBCnav_OBJ_t *nav)
{
    if (!nav)
        return -1;

    memset(&nav->fusionData, 0, sizeof(nav->fusionData));
    memset(&nav->originData, 0, sizeof(nav->originData));

    nav->step = 0;
    nav->tempHeader = 0;
    nav->tempLen = 0;
    nav->tempLoc = 0;
    memset(nav->tempBuf, 0, sizeof(nav->tempBuf));

    return 0;
}

int IBCnav_pushData(IBCnav_OBJ_t *nav, const uint8_t data)
{
    if (!nav)
        return -1;

    int ret = 0;
    switch (nav->step)
    {
    case 0:
        // Find HEAD
        nav->tempHeader = nav->tempHeader >> 8;
        nav->tempHeader |= (data << 8);
        if (nav->tempHeader == IBCnav_HEAD)
        {
            nav->step = 1;
            nav->tempBuf[0] = nav->tempHeader & 0xFF;
            nav->tempBuf[1] = data;
            nav->tempHeader = 0;
        }
        break;
    case 1:
        // Get LEN
        nav->tempLen = data;
        nav->tempBuf[2] = data;
        if (nav->tempLen < IBCnav_MAX_LEN)
        {
            nav->tempLoc = 3;
            nav->step = 2;
        }
        else
        {
            nav->step = 0;
        }
        break;
    case 2:
        // Get DATA of length LEN
        if (nav->tempLoc < (nav->tempLen - 1))
        {
            nav->tempBuf[nav->tempLoc] = data;
            nav->tempLoc++;
        }
        else
        {
            nav->tempBuf[nav->tempLoc] = data;
            nav->tempLoc++;

            // Checksum verification
            if (((nav->tempBuf[nav->tempLen - 1] << 8) | nav->tempBuf[nav->tempLen - 2]) == IBCnav_checksum(nav->tempBuf, nav->tempLen - 2))
            {
                // Assign data
                switch (nav->tempBuf[3])
                {
                case IBCnav_FUSION_ID:
                    memcpy(&nav->fusionData, &nav->tempBuf[0], sizeof(nav->fusionData));
                    break;
                case IBCnav_ORIGIN_ID:
                    memcpy(&nav->originData, &nav->tempBuf[0], sizeof(nav->originData));
                    break;
                default:
                    break;
                }
                ret = 1;
            }

            nav->tempLoc = 0;
            nav->step = 0;
        }
        break;
    default:
        break;
    }
    return ret;
}

void IBCnav_prfOriginData(IBCnav_OBJ_t *nav, int (*println)(const char *fmt, ...))
{
    println("\n\nOriginData:\n");
    println("head: 0x%04x\n", nav->originData.head);
    println("len: %d\n", nav->originData.len);
    println("id: %d\n", nav->originData.id);
    println("count: %d\n", nav->originData.count);

    println("DIAG_STAT: %d\n", nav->originData.DIAG_STAT);

    println("mainIMU_WX: %f\n", nav->originData.mainIMU_WX);
    println("mainIMU_WY: %f\n", nav->originData.mainIMU_WY);
    println("mainIMU_WZ: %f\n", nav->originData.mainIMU_WZ);

    println("mainIMU_FX: %f\n", nav->originData.mainIMU_FX);
    println("mainIMU_FY: %f\n", nav->originData.mainIMU_FY);
    println("mainIMU_FZ: %f\n", nav->originData.mainIMU_FZ);

    println("backupIMU_WX: %f\n", nav->originData.backupIMU_WX);
    println("backupIMU_WY: %f\n", nav->originData.backupIMU_WY);
    println("backupIMU_WZ: %f\n", nav->originData.backupIMU_WZ);

    println("backupIMU_FX: %f\n", nav->originData.backupIMU_FX);
    println("backupIMU_FY: %f\n", nav->originData.backupIMU_FY);
    println("backupIMU_FZ: %f\n", nav->originData.backupIMU_FZ);

    println("MAG_HX: %f\n", nav->originData.MAG_HX);
    println("MAG_HY: %f\n", nav->originData.MAG_HY);
    println("MAG_HZ: %f\n", nav->originData.MAG_HZ);

    println("GPS_sec: %f\n", nav->originData.GPS_sec);
    println("heading: %f\n", nav->originData.heading);
    println("heading_type: %d\n", nav->originData.heading_type);

    println("hor_spd: %f\n", nav->originData.hor_spd);
    println("trk_gnd: %f\n", nav->originData.trk_gnd);
    println("vert_spd: %f\n", nav->originData.vert_spd);

    println("lat: %f\n", nav->originData.lat);
    println("lon: %f\n", nav->originData.lon);
    println("alt: %f\n", nav->originData.hgt);

    println("pos_type: %d\n", nav->originData.pos_type);
    println("PPS_dt:%d\n", nav->originData.PPS_dt);

    println("checksum: %d\n", nav->originData.checksum);
}

void IBCnav_prfFusionData(IBCnav_OBJ_t *nav, int (*println)(const char *fmt, ...))
{
    println("\n\nFusionData:\n");
    println("head: 0x%04x\n", nav->fusionData.head);
    println("len: %d\n", nav->fusionData.len);
    println("id: %d\n", nav->fusionData.id);

    println("counter: %u\n", nav->fusionData.count);
    println("state: 0x%02X\n", nav->fusionData.state);

    println("pitch: %.3f rad\n", nav->fusionData.pitch);
    println("roll: %.3f rad\n", nav->fusionData.roll);
    println("yaw: %.3f rad\n", nav->fusionData.yaw);
    println("yaw_gps: %.3f deg\n", nav->fusionData.yaw_gps);

    println("pitch_rate: %.3f rad/s\n", nav->fusionData.pitch_rate);
    println("roll_rate: %.6f rad/s\n", nav->fusionData.roll_rate);
    println("yaw_rate: %.6f rad/s\n", nav->fusionData.yaw_rate);

    println("lon: %.7lf deg\n", (double)nav->fusionData.lon * 1e-7);
    println("lat: %.7lf deg\n", (double)nav->fusionData.lat * 1e-7);

    println("alt_baro: %.2f m\n", (double)nav->fusionData.alt_baro / 100);
    println("alt_gps: %.2f m\n", (double)nav->fusionData.alt_gps / 100);
    println("alt: %.2f m\n", (double)nav->fusionData.alt / 100);

    println("velocity_x: %.2f m/s\n", nav->fusionData.velocity_x);
    println("velocity_y: %.2f m/s\n", nav->fusionData.velocity_y);
    println("velocity_z: %.2f m/s\n", nav->fusionData.velocity_z);
    println("velocity_air: %.2f m/s\n", nav->fusionData.velocity_air);

    println("accel_x: %.3f m/s^2\n", nav->fusionData.accel_x);
    println("accel_y: %.3f m/s^2\n", nav->fusionData.accel_y);
    println("accel_z: %.3f m/s^2\n", nav->fusionData.accel_z);

    println("satellite_num: %d\n", nav->fusionData.satellite_num);
    println("hdop: %.2f m\n", (double)nav->fusionData.hdop / 100);
    println("vdop: %.2f m\n", (double)nav->fusionData.vdop / 100);

    println("gps_status: %d\n", nav->fusionData.gps_status);
    println("gps_hh: %d\n", nav->fusionData.gps_hh);
    println("gps_mm: %d\n", nav->fusionData.gps_mm);
    println("gps_ss: %d\n", nav->fusionData.gps_ss);

    println("temperature: %d degC\n", nav->fusionData.temperature);
    println("HDT: %.2f deg\n", (double)nav->fusionData.HDT / 10);
    println("HDG_Dev: %.2f deg\n", (double)nav->fusionData.HDG_Dev / 10);

    println("redundancy: %d\n", nav->fusionData.redundancy);
    println("GPS0_DT: %d ms\n", nav->fusionData.GPS0_DT * 100);
    println("GPS1_DT: %d ms\n", nav->fusionData.GPS1_DT * 100);

    println("GPS_vx: %.2f m/s\n", nav->fusionData.GPS_vx);
    println("GPS_vy: %.2f m/s\n", nav->fusionData.GPS_vy);
    println("GPS_vz: %.2f m/s\n", nav->fusionData.GPS_vz);

    println("gps_ms: %u\n", nav->fusionData.gps_ms);
    println("gps_day: %d\n", nav->fusionData.gps_day);
    println("gps_week: %u\n", nav->fusionData.gps_week);
    println("ahrs_state: %d\n", nav->fusionData.ahrs_state);

#if IBCnav_ENABLE_MOBILE_PLATFORM
    println("eastward_dis: %.2f m\n", nav->fusionData.eastward_dis);
    println("northward_dis: %.2f m\n", nav->fusionData.northward_dis);
    println("skyward_dis: %.2f m\n", nav->fusionData.skyward_dis);
    println("std_dev: %.2f m\n", (double)nav->fusionData.std_dev / 100);
    println("std_dev_up: %.2f m\n", (double)nav->fusionData.std_dev_up / 100);

    println("ture_north: %.2f deg\n", (double)nav->fusionData.ture_north / 100);
    println("Ground_speed: %.2f m/s\n", (double)nav->fusionData.Ground_speed / 100);
#endif

    println("Engine_RPM: %u rpm\n", nav->fusionData.Engine_RPM);
    println("check_sum: 0x%04X\n", nav->fusionData.check_sum);
}
